#!/usr/bin/env python
PACKAGE = "fira_esi"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()


gen.add("forward_vel", double_t, 0, "UAV move forward's velocity", 0.15, 0, 1.0)


gen.add("normal_flight_altitude", double_t, 0, "The altitude when UAV taking normal flight", 0.51, -0.5, 2.5)
gen.add("landing_altitude", double_t, 0, "The altitude when UAV taking normal flight", 0.0, -0.5, 2.5)
gen.add("scan_tower_altitude", double_t, 0, "The altitude when scan tower", 1.0, -0.5, 2.5)

gen.add("scan_tower_cycletime", double_t, 0, "The runtime when scan tower. Unit seconds.", 10, 0, 20)
gen.add("scan_tower_square_size", double_t, 0, "The length of each side of the square when scan tower. Unit meters", 1.5, 0, 2.0)


gen.add("pid_p_v2p_x_has_threshold", bool_t, 0, "PID Controller to control x velocity to position use px4 data. Whether has threshold.", False)
gen.add("pid_p_v2p_x_kp", double_t, 0, "PID Controller to control x velocity to position use px4 data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_x_ki", double_t, 0, "PID Controller to control x velocity to position use px4 data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_p_v2p_x_kd", double_t, 0, "PID Controller to control x velocity to position use px4 data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_x_threshold", double_t, 0, "PID Controller to control x velocity to position use px4 data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_p_v2p_y_has_threshold", bool_t, 0, "PID Controller to control y velocity to position use px4 data. Whether has threshold.", False)
gen.add("pid_p_v2p_y_kp", double_t, 0, "PID Controller to control y velocity to position use px4 data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_y_ki", double_t, 0, "PID Controller to control y velocity to position use px4 data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_p_v2p_y_kd", double_t, 0, "PID Controller to control y velocity to position use px4 data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_y_threshold", double_t, 0, "PID Controller to control y velocity to position use px4 data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_p_v2p_z_has_threshold", bool_t, 0, "PID Controller to control z velocity to position use px4 data. Whether has threshold.", False)
gen.add("pid_p_v2p_z_kp", double_t, 0, "PID Controller to control z velocity to position use px4 data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_z_ki", double_t, 0, "PID Controller to control z velocity to position use px4 data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_p_v2p_z_kd", double_t, 0, "PID Controller to control z velocity to position use px4 data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_z_threshold", double_t, 0, "PID Controller to control z velocity to position use px4 data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_p_v2p_yaw_has_threshold", bool_t, 0, "PID Controller to control yaw velocity to position use px4 data. Whether has threshold.", True)
gen.add("pid_p_v2p_yaw_kp", double_t, 0, "PID Controller to control yaw velocity to position use px4 data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_yaw_ki", double_t, 0, "PID Controller to control yaw velocity to position use px4 data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_p_v2p_yaw_kd", double_t, 0, "PID Controller to control yaw velocity to position use px4 data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_p_v2p_yaw_threshold", double_t, 0, "PID Controller to control yaw velocity to position use px4 data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)


gen.add("pid_v_v2p_x_has_threshold", bool_t, 0, "PID Controller to control x velocity to position use vision data. Whether has threshold.", False)
gen.add("pid_v_v2p_x_kp", double_t, 0, "PID Controller to control x velocity to position use vision data. Kp param.", 0.007, 0, 0.05)
gen.add("pid_v_v2p_x_ki", double_t, 0, "PID Controller to control x velocity to position use vision data. Ki param.", 0.0, 0, 0.1)
gen.add("pid_v_v2p_x_kd", double_t, 0, "PID Controller to control x velocity to position use vision data. Kd param.", 0.040, 0, 0.1)
gen.add("pid_v_v2p_x_threshold", double_t, 0, "PID Controller to control x velocity to position use vision data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_v_v2p_y_has_threshold", bool_t, 0, "PID Controller to control y velocity to position use vision data. Whether has threshold.", False)
gen.add("pid_v_v2p_y_kp", double_t, 0, "PID Controller to control y velocity to position use vision data. Kp param.", 0.007, 0, 0.05)
gen.add("pid_v_v2p_y_ki", double_t, 0, "PID Controller to control y velocity to position use vision data. Ki param.", 0.0, 0, 0.1)
gen.add("pid_v_v2p_y_kd", double_t, 0, "PID Controller to control y velocity to position use vision data. Kd param.", 0.040, 0, 0.1)
gen.add("pid_v_v2p_y_threshold", double_t, 0, "PID Controller to control y velocity to position use vision data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_v_v2p_z_has_threshold", bool_t, 0, "PID Controller to control z velocity to position use vision data. Whether has threshold.", False)
gen.add("pid_v_v2p_z_kp", double_t, 0, "PID Controller to control z velocity to position use vision data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_v_v2p_z_ki", double_t, 0, "PID Controller to control z velocity to position use vision data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_v_v2p_z_kd", double_t, 0, "PID Controller to control z velocity to position use vision data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_v_v2p_z_threshold", double_t, 0, "PID Controller to control z velocity to position use vision data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)

gen.add("pid_v_v2p_yaw_has_threshold", bool_t, 0, "PID Controller to control yaw velocity to position use vision data. Whether has threshold.", False)
gen.add("pid_v_v2p_yaw_kp", double_t, 0, "PID Controller to control yaw velocity to position use vision data. Kp param.", 1.0, 0, 6.0)
gen.add("pid_v_v2p_yaw_ki", double_t, 0, "PID Controller to control yaw velocity to position use vision data. Ki param.", 0.0, 0, 6.0)
gen.add("pid_v_v2p_yaw_kd", double_t, 0, "PID Controller to control yaw velocity to position use vision data. Kd param.", 1.0, 0, 6.0)
gen.add("pid_v_v2p_yaw_threshold", double_t, 0, "PID Controller to control yaw velocity to position use vision data. threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)



gen.add("pid_v_d2yr_yaw_rate_has_threshold", bool_t, 0, "PID controller while scaning tower. Depth to yaw rate.", False)
gen.add("pid_v_d2yr_yaw_rate_kp", double_t, 0, "PID controller while scaning tower. Depth to yaw rate. Kp param", 1.0, 0, 6.0)
gen.add("pid_v_d2yr_yaw_rate_ki", double_t, 0, "PID controller while scaning tower. Depth to yaw rate. Ki param", 0.0, 0, 6.0)
gen.add("pid_v_d2yr_yaw_rate_kd", double_t, 0, "PID controller while scaning tower. Depth to yaw rate. Kd param", 0.0, 0, 6.0)
gen.add("pid_v_d2yr_yaw_rate_threshold", double_t, 0, "PID controller while scaning tower. Depth to yaw rate. Kd param threshold param. If has threshold is disable, this param doesn't effect.", 1.0, 0, 6.0)



gen.add("altitude_when_red_gate", double_t, 0, "When running avoidance task, get red gate, the altitude UAV desired", 1.6, -0.5, 2.5)
gen.add("altitude_when_yellow_gate", double_t, 0, "When running avoidance task, get yellow gate, the altitude UAV desired", 0.5, -0.5, 2.5)
gen.add("avoidance_forward_time", double_t, 0, "When running avoidance task, the time UAV forwarding. Unit seconds", 9, 0, 20)


gen.add("altitude_tolerance", double_t, 0, "The tolerance used to determine if the altitude has been reached during the flight", 0.06, 0, 0.5)
gen.add("rotate_yaw_tolerance", double_t, 0, "Tolerance when control yaw to rotate. Unit deg", 5, 0, 30)

gen.add("goto_point_x_tolerance", double_t, 0, "Go to point x tolerance. Unit meter", 0.1, 0, 1)
gen.add("goto_point_y_tolerance", double_t, 0, "Go to point y tolerance. Unit meter", 0.1, 0, 1)

gen.add("qr_line_y_tolerance", double_t, 0, "The line_y_tolerance when adjust pose on QR. Unit pixel", 40, 0, 100)
gen.add("qr_line_yaw_tolerance", double_t, 0, "The line_yaw_tolerance when adjust pose on QR. Unit rad", 0.2, 0, 1)


# TODO
gen.add("blue_h_offset_x_tolerance", double_t, 0, "When landing, the tolerance of bule H vision offset x", 9, 0, 100)
gen.add("blue_h_offset_y_tolerance", double_t, 0, "When landing, the tolerance of bule H vision offset y", 9, 0, 100)

gen.add("red_x_offset_x_tolerance", double_t, 0, "When landing, the tolerance of bule H vision offset x", 9, 0, 100)
gen.add("red_x_offset_y_tolerance", double_t, 0, "When landing, the tolerance of bule H vision offset y", 9, 0, 100)


gen.add("qr_offset_x_tolerance", double_t, 0, "Tolerance when hovering on QR, x", 9, 0, 100)
gen.add("qr_offset_y_tolerance", double_t, 0, "Tolerance when hovering on QR, y", 9, 0, 100)

gen.add("tf_break_duration", double_t, 0, "When tf query is time out, wait tf_break_duration time", 0.2, 0, 1.0)

gen.add("navigation_per_sustain_time", int_t, 0, "NAVIGATION Task per sustain time", 15, 0, 100)
gen.add("judge_achieve_counter_threshold", int_t, 0, "Judge whether achieve counter, Unit beat", 15, 0, 100)
gen.add("open_claw_msg_send_frequency", int_t, 0, "Open claw message send frequency", 70, 0, 100)

gen.add("cycle_moving_radius", double_t, 0, "Radius when cycle moving", 0.2, 0, 2.0)
gen.add("cycle_moving_linear_vel", double_t, 0, "Linear velocity when cycle moving", 0.4, 0, 2.0)

gen.add("local_frame_id", str_t, 0, "The parent frame id when tf listener listening", "local_origin")
gen.add("body_frame_id", str_t, 0, "The child frame id when tf listener listening", "fcu")
# gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
# gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
# gen.add("bool_param",   bool_t,   0, "A Boolean parameter",  True)

# size_enum = gen.enum([ gen.const("Small",      int_t, 0, "A small constant"),
#                       gen.const("Medium",     int_t, 1, "A medium constant"),
#                       gen.const("Large",      int_t, 2, "A large constant"),
#                       gen.const("ExtraLarge", int_t, 3, "An extra large constant")], "An enum to set size")

# gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, # edit_method=size_enum)

exit(gen.generate(PACKAGE, "fira_esi", "fira_esi_dynamic_cfg"))
